#!/usr/bin/env python

import rospy, os
from task_planner.srv import *
from task_planner.msg import *
from std_srvs.srv import *

env = None
status_topic = None
i = 0

############# CLASS ###################
class Environment_class:
	
	def __init__(self):
		self.p_InHome = True			#settata dal robot
		self.p_InP1 = False				#settata dal robot
		self.p_InP2 = False				#settata dal robot
		self.p_Call1 = False			#settata dalla telecamera
		self.p_Call2 = False			#settata dalla telecamera
		self.p_Command = False			#settata dal robot
		self.p_HomeCommand = False		#settata dal robot
		self.p_Available = False		#settata dal robot
		self.a_StandBy = True
		self.a_GoInP1 = False
		self.a_GoInP2 = False
		self.a_ExecCommand = False
		self.a_GoHome1 = False
		self.a_GoHome2 = False
		
			
	def restoreActions(self):
		self.a_StandBy = False
		self.a_GoInP1 = False
		self.a_GoInP2 = False
		self.a_ExecCommand = False
		self.a_GoHome1 = False
		self.a_GoHome2 = False
		
	def __str__(self):
		ret = 'STATUS:\n'
		ret = ret + 'p_InHome: ' + str(self.p_InHome) + '\n'
		ret = ret + 'p_InP1: ' + str(self.p_InP1) + '\n'
		ret = ret + 'p_InP2: ' + str(self.p_InP2) + '\n'
		ret = ret + 'p_Call1: ' + str(self.p_Call1) + '\n'
		ret = ret + 'p_Call2: ' + str(self.p_Call2) + '\n'
		ret = ret + 'p_Command: ' + str(self.p_Command) + '\n'
		ret = ret + 'p_HomeCommend: ' + str(self.p_HomeCommand) + '\n'
		ret = ret + 'p_Available: ' + str(self.p_Available) + '\n'
		ret = ret + ' ------------- \n'
		return ret

################## SERVICE HANDLERS ################################
	
def handle_set_Call1(req):
	if env.p_InHome and env.a_StandBy:
		rospy.wait_for_service('/magabot/GoInP1')
		try:
			s = rospy.ServiceProxy('/magabot/GoInP1', Empty)
			env.restoreActions()
			env.a_GoInP1 = True
			env.p_Call1 = True
			s()
		except rospy.ServiceException, e:
			print "error service /magabot/GoInP1"
			return EmptyResponse()
		env.p_InHome = False
		env.p_InP1 = True
		env.p_InP2 = False
		env.p_Available = True
		env.p_Call1 = False
	print 'call1'
	print env
	return EmptyResponse()
	
def handle_set_Call2(req):
	if env.p_InHome and env.a_StandBy:
		rospy.wait_for_service('/magabot/GoInP2')
		try:
			s1 = rospy.ServiceProxy('/magabot/GoInP2', Empty)
			env.restoreActions()
			env.a_GoInP2 = True
			env.p_Call2 = True
			s1()
		except rospy.ServiceException, e:
			print "error service /magabot/GoInP2"
			return EmptyResponse()
		env.p_InHome = False
		env.p_InP1 = False
		env.p_InP2 = True
		env.p_Available = True
		env.p_Call2 = False
	print 'call2'
	print env
	return EmptyResponse()
	
def handle_set_Command(req):
	if (env.p_InP1 or env.p_InP2) and env.p_Available and (env.a_GoInP1 or env.a_GoInP2):
		rospy.wait_for_service('/magabot/ExecCommand')
		try:
			s1 = rospy.ServiceProxy('/magabot/ExecCommand', Empty)
			env.restoreActions()
			env.a_ExecCommand = True
			env.p_Available = False
			env.p_Command = True
			s1()
		except rospy.ServiceException, e:
			print "error service /magabot/execCommand"
			return EmptyResponse()
		env.p_Available = True
		env.p_Command = False
	print 'command'
	print env
	return EmptyResponse()
		
def handle_set_HomeCommand(req):
	if env.p_InP1 and env.p_Available and env.a_ExecCommand:
		rospy.wait_for_service('/magabot/GoHome1')
		try:
			s1 = rospy.ServiceProxy('/magabot/GoHome1', Empty)
			env.restoreActions()
			env.a_GoHome1 = True
			env.p_Available = False
			env.p_HomeCommand = True
			s1()
		except rospy.ServiceException, e:
			print "error service /magabot/GoHome1"
			return EmptyResponse()
		env.p_InHome = True
		env.p_InP1 = False
		env.p_InP2 = False
		env.p_HomeCommand = False
		env.a_StandBy = True
	elif env.p_InP2 and env.p_Available and env.a_ExecCommand:
		rospy.wait_for_service('/magabot/GoHome2')
		try:
			s1 = rospy.ServiceProxy('/magabot/GoHome2', Empty)
			env.restoreActions()
			env.a_GoHome2 = True
			env.p_Available = False
			env.p_HomeCommand = True
			s1()
		except rospy.ServiceException, e:
			print "error service /magabot/GoHome2"
			return EmptyResponse()
		env.p_InHome = True
		env.p_InP1 = False
		env.p_InP2 = False
		env.p_HomeCommand = False
		env.a_StandBy = True
	print 'home command'
	print env
	return EmptyResponse()
	
#probabilmente inutile
def handle_set_Available(req):
	env.update(p_HomeCommand = req.data)
	return SetBoolResponse()
	
######################## MESSAGES ###################################
	
def publish_status():
	global status_topic
	global i 
	i = i+1
	print "- Publish on topic environment/status: ", i
	s = Environment() #da modificare
	s.p_InHome = env.p_InHome				
	s.p_InP1 = env.p_InP1					
	s.p_InP2 = env.p_InP2					
	s.p_Call1 = env.p_Call1					
	s.p_Call2 = env.p_Call2					
	s.p_Command = env.p_Command				
	s.p_HomeCommand = env.p_HomeCommand		
	s.p_Available = env.p_Available			
	status_topic.publish(s)

########################## MAIN #####################################		

def start_interface():
	global status_topic
	os.system("clear") 
	print " + Help Request Interface: START \n"
	
	rospy.init_node('help_request_interface')
	
	rospy.Service('/environment/Call1', Empty, handle_set_Call1)
	rospy.Service('/environment/Call2', Empty, handle_set_Call2)
	rospy.Service('/environment/Command', Empty, handle_set_Command)
	rospy.Service('/environment/HomeCommand', Empty, handle_set_HomeCommand)
	
	#status_topic = rospy.Publisher('/environment/help_request_status', HelpRequestStatus, latch = True)
	
	#publish_status()
	rospy.spin()
	
if __name__ == '__main__':
	env = Environment_class()
	start_interface()
